#include "btBulletCollisionCommon.h"
#include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.h"
#include "LinearMath/btAlignedObjectArray.h"
#include <BulletCollision/CollisionShapes/btCompoundShape.h>
#include <BulletCollision/CollisionShapes/btConvexHullShape.h>
#include <BulletCollision/CollisionDispatch/btGhostObject.h>
#include <BulletDynamics/Dynamics/btDynamicsWorld.h>
#include <BulletCollision/CollisionShapes/btBoxShape.h>
#include <BulletCollision/CollisionShapes/btSphereShape.h>
#include <BulletCollision/CollisionShapes/btUniformScalingShape.h>
#include "BulletDynamics/ConstraintSolver/btConstraintSolver.h"
#include "btBulletDynamicsCommon.h"
#include <vector>
#include <string>
#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
#include "cpp/vectormath_aos.h"
#include "utils.hpp"
#ifndef _BT_INTERFACE_
#define _BT_INTERFACE_

//May be someday it wiil be documented. But not now since it is not stable and commentaries may mislead you.

namespace BT_Interface
{
using namespace  Vectormath::Aos;

class World;
struct Object;
struct Query
{
    std::vector<Object*>  visible;
};

struct Ghost
{
    Ghost();
    ~Ghost();
    std::string name;
    btCompoundShape* shape;
    btConvexHullShape *convex_shape;
    btPairCachingGhostObject* m_ghostObject;
    btOverlappingPairCallback*	m_ghostPairCallback;
    void build_frustum(float aspect,float left,float right,float top, float bottom,float znear,float zfar);
};

struct Object
{
    std::string name;
    btRigidBody* body;
    btCollisionShape* shape;
    btDefaultMotionState* motionState;
    Object();
    Object(void *p);
    ~Object();
    void release();
    void make_heightmap(Heightmap &hmap,const btVector3& stride);
    void make_box(float mass,const btVector3& size,const btVector3& pos,const btQuaternion& orient);
    void make_plane(const btVector3& pos, const btVector3& normal,float smthng);
    void make_compound(float mass,const btVector3& size,const btVector3& pos,const btQuaternion& orient);
    void add_box(const btVector3 &pos,const btVector3& normal,float smthng);
    void set_user_object(void *);
    void get_user_object(void *);
    void *user_data;

};





struct Vehicle
{
    btRaycastVehicle::btVehicleTuning m_tuning;
    btVehicleRaycaster*	m_vehicleRayCaster;
    btRaycastVehicle*	m_vehicle;
    Object body;
    World *world;
    std::vector<TestbedGL::Matrix4GL>wheel_matrices;

    btVector3 wheelDirectionCS0,wheelAxleCS;
    btScalar gVehicleSteering,steeringClamp,wheelRadius,wheelWidth,wheelFriction,suspensionStiffness,suspensionDamping;
    btScalar  suspensionCompression,rollInfluence,suspensionRestLength;

    void set_wheel_common_parameters()
    {
        wheelDirectionCS0 = btVector3(0,-1,0);
        wheelAxleCS = btVector3(-1,0,0);
        gVehicleSteering = 0.f;
        steeringClamp = 0.3f;
        wheelRadius = 1.0;
        wheelWidth = 0.2;
        wheelFriction = 0.8f;
        suspensionStiffness = 40.9f;
        suspensionDamping = 18.0f;
        suspensionCompression = -2.2f;
        rollInfluence = 0.0f;
        suspensionRestLength = 0.5;
    }


    void add_vehicle_to_world(World &world);
    void set_body(float weight,const btVector3& size,const btVector3& pos,const btQuaternion& dir);
    void add_wheel(std::string name,const btVector3& pos,btScalar radius, btScalar width, bool front)
    {
        m_vehicle->addWheel(pos,wheelDirectionCS0,wheelAxleCS,suspensionRestLength,radius,m_tuning,front);
        btWheelInfo& wheel = m_vehicle->getWheelInfo(m_vehicle->getNumWheels()-1);

        wheel.m_suspensionStiffness = suspensionStiffness;
        wheel.m_wheelsDampingRelaxation = suspensionDamping;
        wheel.m_wheelsDampingCompression = suspensionCompression;
        wheel.m_frictionSlip = wheelFriction;
        wheel.m_rollInfluence = rollInfluence;

        wheel_matrices.push_back(TestbedGL::Matrix4GL());
    }

    void update_wheels()
    {
        float m[16];
        for(int i = 0; i <m_vehicle->getNumWheels();i++ )
        {
            m_vehicle->updateWheelTransform(i,true);
            m_vehicle->getWheelInfo(i).m_worldTransform.getOpenGLMatrix(m);
            wheel_matrices[i].set_matrix(m);
        }
    }

    void update_params(float steering, float engine, float brakes)
    {
        for(int i = 0; i <m_vehicle->getNumWheels();i++ )
        {
          if (m_vehicle->getWheelInfo(i).m_bIsFrontWheel)
                m_vehicle->setSteeringValue(steering,i);
           m_vehicle->applyEngineForce(engine,i);
           m_vehicle->setBrake(brakes,i);
        }
    }

};

class World
{
private:
    //btAlignedObjectArray<btCollisionShape*>	m_collisionShapes;
    btBroadphaseInterface*	                m_broadphase;
    btCollisionDispatcher*	                m_dispatcher;
    btConstraintSolver*	                    m_solver;
    btDefaultCollisionConfiguration*        m_collisionConfiguration;
    std::vector<Object*>					managed_objects;
    btVector3								ray;

public:
    btDynamicsWorld*		                m_dynamicsWorld;

    //Frustum stuff
    World();
    virtual ~World();
    void query_ghost(Ghost &qry,Query &qr);
    std::string query_ray(const btVector3& pos,const btVector3& dir);
    void update_world(long time);
    void add_ghost(Ghost &ghost);
    void add_object(Object &object);
    void add_vehicle(Vehicle &vehicle);
    void set_height_map_ground(std::string hmap);
    void do_step(float sz);
};
};
#endif
